<?xml version="1.0" ?>
<%
  # Tricycle with spherical wheels and front steering
  # Consists of triangular frame with 1 steerable
  # and 2 non-steerable wheels
  # SI units (length in meters)

  # Geometry
  wheel_radius    = 0.15
  wheelbase       = 6 * wheel_radius
  half_track      = wheelbase * Math.tan(30 * Math::PI/180)
  caster_angle    = 10 * Math::PI / 180.0
  fork_length     = 1.5 * wheel_radius * Math.sqrt(2)
  headtube_length = 0.5 * fork_length
  front_steer_L   = headtube_length + fork_length
  front_steer_dx  = front_steer_L * Math.sin(caster_angle)
  front_steer_dz  = front_steer_L * Math.cos(caster_angle)
  rear_steer_dx   = wheelbase - front_steer_dx
  rear_steer_L    = Math.sqrt(rear_steer_dx**2 + front_steer_dz**2)
  frame_pitch     = Math.atan2(front_steer_dz, rear_steer_dx)
  frame_dx   = rear_steer_L
  frame_dy   = half_track
  frame_dz   = 0.4*wheel_radius
  frame_z0   = wheel_radius + front_steer_dz / 2.0
  frame_angle = 20 * Math::PI/180
  fork_dx = 0.5 * wheel_radius
  fork_dy = 2*fork_length
  fork_dz = front_steer_L
  fork_offset = fork_length - 0.5*front_steer_L
  wheel_x0     = rear_steer_dx
  wheel_y0     = half_track
  steer_limit  = 55 * Math::PI / 180.0

  rear_wheel_locations = {
    "rear_left"   => {:x0 => -wheel_x0, :y0 => wheel_y0 },
    "rear_right"  => {:x0 => -wheel_x0, :y0 => -wheel_y0 },
  }

  # inertia
  frame_mass = 10
  frame_ixx  = frame_mass/12.0 * (frame_dy**2 + frame_dz**2)
  frame_iyy  = frame_mass/12.0 * (frame_dz**2 + frame_dx**2)
  frame_izz  = frame_mass/12.0 * (frame_dx**2 + frame_dy**2)
  # frame c.g. offset from center of box
  frame_cgx  = frame_dx*0.0
  frame_cgy  = 0 
  frame_cgz  = 0 
  frame_mass = 10
  fork_mass = frame_mass / 3
  fork_ixx  = fork_mass/12.0 * (fork_dy**2 + fork_dz**2)
  fork_iyy  = fork_mass/12.0 * (fork_dz**2 + fork_dx**2)
  fork_izz  = fork_mass/12.0 * (fork_dx**2 + fork_dy**2)
  wheel_mass   = 0.5
  wheel_ixx    = 0.4 * wheel_mass * wheel_radius**2
  wheel_iyy    = wheel_ixx
  wheel_izz    = wheel_ixx

  # wheel surface properties
  wheel_kp = 1e8
  wheel_kd = 1
  wheel_min_depth = 0.0005
%>
<sdf version="1.6">
  <model name="trisphere_cycle">
    <link name="frame">
      <pose><%= -rear_steer_dx / 2 %> 0 <%= frame_z0 %>  0 <%= -frame_pitch %> 0</pose>
      <inertial>
        <pose><%= frame_cgx %> <%= frame_cgy %> <%= frame_cgz %>  0 0 0</pose>
        <mass><%= frame_mass %></mass>
        <inertia>
          <ixx><%= frame_ixx %></ixx>
          <iyy><%= frame_iyy %></iyy>
          <izz><%= frame_izz %></izz>
          <ixy>0</ixy>
          <ixz>0</ixz>
          <iyz>0</iyz>
        </inertia>
      </inertial>
<%  ["collision", "visual"].each do |elem| %>
      <<%= elem %> name="axle_<%= elem %>">
        <pose><%= -0.5*frame_dx %> 0 0  <%= Math::PI/2 %> 0 0</pose>
        <geometry>
          <cylinder>
            <length><%= 2 * half_track %></length>
            <radius><%= 0.2 * wheel_radius %></radius>
          </cylinder>
        </geometry>
<%    if elem == "visual" %>
        <material>
          <script>
            <uri>file://media/materials/scripts/gazebo.material</uri>
            <name>Gazebo/Grey</name>
          </script>
        </material>
<%    end %>
      </<%= elem %>>
<%  end %>
<%  ["collision", "visual"].each do |elem| %>
      <<%= elem %> name="frame_left_<%= elem %>">
        <pose>0 <%= 0.5*frame_dx*Math.tan(frame_angle) %> 0  0 <%= Math::PI/2 %> <%= -frame_angle %></pose>
        <geometry>
          <cylinder>
            <length><%= frame_dx / Math.cos(frame_angle) %></length>
            <radius><%= 0.2 * wheel_radius %></radius>
          </cylinder>
        </geometry>
<%    if elem == "visual" %>
        <material>
          <script>
            <uri>file://media/materials/scripts/gazebo.material</uri>
            <name>Gazebo/Grey</name>
          </script>
        </material>
<%    end %>
      </<%= elem %>>
<%  end %>
<%  ["collision", "visual"].each do |elem| %>
      <<%= elem %> name="frame_right_<%= elem %>">
        <pose>0 <%= -0.5*frame_dx*Math.tan(frame_angle) %> 0  0 <%= Math::PI/2 %> <%= frame_angle %></pose>
        <geometry>
          <cylinder>
            <length><%= frame_dx / Math.cos(frame_angle) %></length>
            <radius><%= 0.2 * wheel_radius %></radius>
          </cylinder>
        </geometry>
<%    if elem == "visual" %>
        <material>
          <script>
            <uri>file://media/materials/scripts/gazebo.material</uri>
            <name>Gazebo/Grey</name>
          </script>
        </material>
<%    end %>
      </<%= elem %>>
<%  end %>
    </link>
    <link name="fork">
      <pose><%= front_steer_dx / 2 %> 0 <%= frame_z0 %>  0 <%= -caster_angle %> 0</pose>
      <inertial>
        <mass><%= fork_mass %></mass>
        <inertia>
          <ixx><%= fork_ixx %></ixx>
          <iyy><%= fork_iyy %></iyy>
          <izz><%= fork_izz %></izz>
          <ixy>0</ixy>
          <ixz>0</ixz>
          <iyz>0</iyz>
        </inertia>
      </inertial>
<%  ["collision", "visual"].each do |elem| %>
      <<%= elem %> name="handlebars_<%= elem %>">
        <pose>0 0 <%= 2*headtube_length + fork_offset %>  <%= Math::PI/2 %> 0 0</pose>
        <geometry>
          <cylinder>
            <length><%= 2*fork_length %></length>
            <radius><%= 0.5*fork_dx %></radius>
          </cylinder>
        </geometry>
<%    if elem == "visual" %>
        <material>
          <script>
            <uri>file://media/materials/scripts/gazebo.material</uri>
            <name>Gazebo/Grey</name>
          </script>
        </material>
<%    end %>
      </<%= elem %>>
<%  end %>
<%  ["collision", "visual"].each do |elem| %>
      <<%= elem %> name="headtube_<%= elem %>">
        <pose>0 0 <%= headtube_length + fork_offset %>  0 0 0</pose>
        <geometry>
          <cylinder>
            <length><%= 2*headtube_length %></length>
            <radius><%= 0.5*fork_dx %></radius>
          </cylinder>
        </geometry>
<%    if elem == "visual" %>
        <material>
          <script>
            <uri>file://media/materials/scripts/gazebo.material</uri>
            <name>Gazebo/Grey</name>
          </script>
        </material>
<%    end %>
      </<%= elem %>>
<%  end %>
<%  ["collision", "visual"].each do |elem| %>
      <<%= elem %> name="spindle_<%= elem %>">
        <pose>0 0 <%= -fork_length + fork_offset %>  <%= Math::PI/2 %> 0 0</pose>
        <geometry>
          <cylinder>
            <length><%= 2 * fork_length %></length>
            <radius><%= 0.1 * wheel_radius %></radius>
          </cylinder>
        </geometry>
<%    if elem == "visual" %>
        <material>
          <script>
            <uri>file://media/materials/scripts/gazebo.material</uri>
            <name>Gazebo/Grey</name>
          </script>
        </material>
<%    end %>
      </<%= elem %>>
<%  end %>
<%  ["collision", "visual"].each do |elem| %>
      <<%= elem %> name="fork_left_<%= elem %>">
        <pose>0 <%= fork_length / 2 %> <%= -fork_length/2 + fork_offset %>  <%= Math::PI/4 %> 0 0</pose>
        <geometry>
          <cylinder>
            <length><%= fork_length * Math.sqrt(2) %></length>
            <radius><%= 0.5*fork_dx %></radius>
          </cylinder>
        </geometry>
<%    if elem == "visual" %>
        <material>
          <script>
            <uri>file://media/materials/scripts/gazebo.material</uri>
            <name>Gazebo/Grey</name>
          </script>
        </material>
<%    end %>
      </<%= elem %>>
<%  end %>
<%  ["collision", "visual"].each do |elem| %>
      <<%= elem %> name="fork_right_<%= elem %>">
        <pose>0 <%= -fork_length / 2 %> <%= -fork_length/2 + fork_offset %>  <%= -Math::PI/4 %> 0 0</pose>
        <geometry>
          <cylinder>
            <length><%= fork_length * Math.sqrt(2) %></length>
            <radius><%= 0.5*fork_dx %></radius>
          </cylinder>
        </geometry>
<%    if elem == "visual" %>
        <material>
          <script>
            <uri>file://media/materials/scripts/gazebo.material</uri>
            <name>Gazebo/Grey</name>
          </script>
        </material>
<%    end %>
      </<%= elem %>>
<%  end %>
    </link>
    <link name="wheel_front">
      <pose><%= front_steer_dx %> 0 <%= wheel_radius %>  0 0 0</pose>
      <inertial>
        <mass><%= wheel_mass %></mass>
        <inertia>
          <ixx><%= wheel_ixx %></ixx>
          <iyy><%= wheel_iyy %></iyy>
          <izz><%= wheel_izz %></izz>
          <ixy>0</ixy>
          <ixz>0</ixz>
          <iyz>0</iyz>
        </inertia>
      </inertial>
      <collision name="collision">
        <geometry>
          <sphere>
            <radius><%= wheel_radius %></radius>
          </sphere>
        </geometry>
        <surface>
          <contact>
            <ode>
              <kp><%= wheel_kp %></kp>
              <kd><%= wheel_kd %></kd>
              <min_depth><%= wheel_min_depth %></min_depth>
            </ode>
          </contact>
        </surface>
      </collision>
      <visual name="visual">
        <geometry>
          <sphere>
            <radius><%= wheel_radius %></radius>
          </sphere>
        </geometry>
        <material>
          <script>
            <uri>file://media/materials/scripts/gazebo.material</uri>
            <name>Gazebo/Black</name>
          </script>
        </material>
      </visual>
    </link>
    <joint name="wheel_front_steer" type="revolute">
      <parent>frame</parent>
      <child>fork</child>
      <axis>
        <xyz>0 0 1</xyz>
        <limit>
          <lower><%= -steer_limit %></lower>
          <upper><%=  steer_limit %></upper>
        </limit>
      </axis>
    </joint>
    <joint name="wheel_front_spin" type="revolute">
      <parent>fork</parent>
      <child>wheel_front</child>
      <axis>
        <xyz>0 1 0</xyz>
      </axis>
    </joint>
<%
  rear_wheel_locations.keys.each do |k|
    x0 = rear_wheel_locations[k][:x0]
    y0 = rear_wheel_locations[k][:y0]
%>
    <link name="wheel_<%= k %>">
      <pose><%= x0 %> <%= y0 %> <%= wheel_radius %>  0 0 0</pose>
      <inertial>
        <mass><%= wheel_mass %></mass>
        <inertia>
          <ixx><%= wheel_ixx %></ixx>
          <iyy><%= wheel_iyy %></iyy>
          <izz><%= wheel_izz %></izz>
          <ixy>0</ixy>
          <ixz>0</ixz>
          <iyz>0</iyz>
        </inertia>
      </inertial>
      <collision name="collision">
        <geometry>
          <sphere>
            <radius><%= wheel_radius %></radius>
          </sphere>
        </geometry>
        <surface>
          <contact>
            <ode>
              <kp><%= wheel_kp %></kp>
              <kd><%= wheel_kd %></kd>
              <min_depth><%= wheel_min_depth %></min_depth>
            </ode>
          </contact>
        </surface>
      </collision>
      <visual name="visual">
        <geometry>
          <sphere>
            <radius><%= wheel_radius %></radius>
          </sphere>
        </geometry>
        <material>
          <script>
            <uri>file://media/materials/scripts/gazebo.material</uri>
            <name>Gazebo/Black</name>
          </script>
        </material>
      </visual>
    </link>
    <joint name="wheel_<%= k %>_spin" type="revolute">
      <parent>frame</parent>
      <child>wheel_<%= k %></child>
      <axis>
        <xyz>0 1 0</xyz>
      </axis>
    </joint>
<%
  end
%>
  </model>
</sdf>
